#include <stdlib.h>
#include "PScontrol.h"
#include <unistd.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "motor.h"
#include "servor_control.h"
#include <hi_uart.h>
#include <iot_uart.h>
#include "iot_gpio_ex.h"
/*********************************************************
      
**********************************************************/	 
#define GPIO2 2
#define GPIO8 8
#define GPIO11 11
#define GPIO12 12
#define GPIOFUNC 0

#define DELAY_TIME  usleep(5); 
#define DO_H  gpio_control(GPIO8, IOT_GPIO_VALUE1);
#define DO_L  gpio_control(GPIO8, IOT_GPIO_VALUE0);
#define CS_H  gpio_control(GPIO11, IOT_GPIO_VALUE1);
#define CS_L  gpio_control(GPIO11, IOT_GPIO_VALUE0);
#define CLK_H  gpio_control(GPIO12, IOT_GPIO_VALUE1);
#define CLK_L  gpio_control(GPIO12, IOT_GPIO_VALUE0);
#define DI  gpio_control_INPUT(GPIO2);

float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;         //PS2相关变量
uint16_t key, key_bak;
unsigned char flag_scan_ps2 = 0;
int Handkey;	// 按键值读取，零时存储。
int Comd[2]={0x01,0x42};	//开始命令。请求数据
int Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //数据存储数组
int MASK[]={
    PSB_SELECT,
    PSB_L3,
    PSB_R3 ,
    PSB_START,
    PSB_PAD_UP,
    PSB_PAD_RIGHT,
    PSB_PAD_DOWN,
    PSB_PAD_LEFT,
    PSB_L2,
    PSB_R2,
    PSB_L1,
    PSB_R1 ,
    PSB_GREEN,
    PSB_RED,
    PSB_BLUE,
    PSB_PINK
	};	
uint16_t LX_AD;
uint16_t LY_AD;
uint16_t RX_AD;
uint16_t RY_AD;
uint8_t ps2_mode=0;
char menu=1;
// void gpio_config()
// {
//     gpio_control_INPUT(GPIO2);
//     gpio_control_pull(GPOIO8, HI_IO_PULL_UP);
//     gpio_control_pull(GPOIO11, HI_IO_PULL_UP);
//     gpio_control_pull(GPOIO12, HI_IO_PULL_UP); 
// }

// static void Uart1GpioCOnfig(void)
// {
//     IoSetFunc(HI_IO_NAME_GPIO_0, IOT_IO_FUNC_GPIO_0_UART1_TXD);
//     IoSetFunc(HI_IO_NAME_GPIO_1, IOT_IO_FUNC_GPIO_1_UART1_RXD);
// }





//向手柄发送命令
void PS2_Cmd(int CMD)
{
	volatile uint16_t ref=0x01;
	Data[1] = 0;
	for(ref=0x01;ref<0x0100;ref<<=1)
	{
		if(ref&CMD)
		{
			DO_H;                   //输出以为控制位
		}
		else DO_L;

		CLK_H;                        //时钟拉高
		//delay_us(50);
		usleep(50);
		CLK_L;
		//delay_us(50);
		usleep(50);
		CLK_H;
		if(gpio_control_INPUT(GPIO2))
			Data[1] = ref|Data[1];
	}
}
//判断是否为红灯模式
//返回值；0，红灯模式
//		  其他，其他模式
int PS2_RedLight(void)
{
	CS_L;
	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据
	CS_H;
	if( Data[1] == 0X73)   return 0 ;
	else return 1;

}
//读取手柄数据
void PS2_ReadData(void)
{
	volatile int byte=0;
	volatile uint16_t ref=0x01;

	CS_L;

	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据

	for(byte=2;byte<9;byte++)          //开始接受数据
	{
		for(ref=0x01;ref<0x100;ref<<=1)
		{
			CLK_H;
			CLK_L;
			//delay_us(50);
			usleep(50);
			CLK_H;
		      if(gpio_control_INPUT(GPIO2))
		      Data[byte] = ref|Data[byte];
		}
       //delay_us(50);
		   usleep(50);
	}
	CS_H;	
}

int PS2_DataKey()
{
	static  uint8_t x=1;
	int index;
	PS2_ClearData();
	PS2_ReadData();

	Handkey=(Data[4]<<8)|Data[3];     //这是16个按键  按下为0， 未按下为1
	for(index=0;index<16;index++)
	{	    
		if((Handkey&(1<<(MASK[index]-1)))==0)
		{
			x=(Handkey&(1<<(MASK[index]-1)));
			//Delay_ms(1000);  
		//	printf("  \r\n !%d! \r\n",x);
			return index+1;
		}
		
	}

	return 0;          //没有任何按键按下
}

//得到一个摇杆的模拟量	 范围0~255
int PS2_AnologData(int button)
{
	return Data[button];
}

//清除数据缓冲区
void PS2_ClearData()
{
	int a;
	for(a=0;a<9;a++)
		Data[a]=0x00;
}
static void Uart1GpioCOnfig(void)
{
    IoSetFunc(HI_IO_NAME_GPIO_11, IOT_IO_FUNC_GPIO_11_UART2_TXD);
    IoSetFunc(HI_IO_NAME_GPIO_12, IOT_IO_FUNC_GPIO_12_UART2_RXD);

}
void Uart_config()
{
uint32_t ret = 0;

    IotUartAttribute uartAttr = {
        .baudRate = 115200, /* baudRate: 115200 */
        .dataBits = 8, /* dataBits: 8bits */
        .stopBits = 1, /* stop bit */
        .parity = 0,
    };
    ret = IoTUartInit( HI_UART_IDX_2, &uartAttr);
    if (ret != HI_ERR_SUCCESS) {
        printf("Failed to init pscontrol_uart! Err code = %d\n", ret);
        return;
    }

}


void scan_ps2(void)
{
	if (flag_scan_ps2)//
	{
		flag_scan_ps2 = 0;
		key = PS2_DataKey();
	    LX_AD=PS2_AnologData(PSS_LX);
		RY_AD=PS2_AnologData(PSS_RY);	
		//UART_Put_Inf("LY_AD:",LY_AD);
		ps2_mode=PS2_RedLight();
		if(ps2_mode==0)
		{
			if(key==PSB_SELECT)
			{
				if(key_bak == key)return;
		    	key_bak=key;
				menu=0;
			}
			if(key==PSB_START)
			{
				if(key_bak == key)return;
		  	   key_bak=key;
				menu=1;
			}
			if(menu==0)
			{
				switch(key)
				{
					case PSB_PAD_UP:break; 
					case PSB_PAD_DOWN:break;
					case PSB_PAD_LEFT:break;
					case PSB_PAD_RIGHT:break;
		
					case PSB_TRIANGLE:break; 
					case PSB_CROSS:break;
					case PSB_PINK:break; 
					case PSB_CIRCLE:break;

					case PSB_L1:;car_forward();break; 
					case PSB_L2:;car_backward();break;
					case PSB_R1:break; 
					case PSB_R2:break;
					default:break;
				}
				//LY_AD=PS2_AnologData(PSS_LY);
		    //RY_AD=PS2_AnologData(PSS_RY);					
				if(LX_AD>0 && LX_AD<=255)   
		              car_forward();
        }

			 if(RY_AD>110 && RY_AD<=146)  
				{
					set_angle(0);
				}
				else if(RY_AD>0 && RY_AD<=110)
				{
					engine_turn_left_45();
				}
				else if(RY_AD>146 && RY_AD<=255)
				{
					engine_turn_right_45();
				}
					
		}
	}
}
